
#include "pid.h"
#include "can.h"
#include "usart.h"
#include "math.h"

PID PTZ_Motor_PID_Position_P, PTZ_Motor_PID_Speed_P, PTZ_Motor_PID_Torque_P;
PID PTZ_Motor_PID_Position_Y, PTZ_Motor_PID_Speed_Y, PTZ_Motor_PID_Torque_Y;
PID Hit_position, Hit_speed;
void PID_Init()
{

	//--------------------------云台yaw-----------------------------------------------------

	PTZ_Motor_PID_Position_Y.error_last = 0;
	PTZ_Motor_PID_Position_Y.error_now = 0;
	PTZ_Motor_PID_Position_Y.Kd = 0;	 //������
	PTZ_Motor_PID_Position_Y.Ki = 0.001; //�������һ�����г����������л��𵴣�
	PTZ_Motor_PID_Position_Y.Kp = 1000;  //��Ӧ�ٶ�
	PTZ_Motor_PID_Position_Y.limit=29000;

	//	PTZ_Motor_PID_Speed_Y.error_inter=0;
	//	PTZ_Motor_PID_Speed_Y.error_last=0;
	//	PTZ_Motor_PID_Speed_Y.error_now=0;
	//	PTZ_Motor_PID_Speed_Y.Kd=0;
	//	PTZ_Motor_PID_Speed_Y.Ki=0;
	//	PTZ_Motor_PID_Speed_Y.Kp=200;

	PTZ_Motor_PID_Torque_Y.error_inter = 0;
	PTZ_Motor_PID_Torque_Y.error_last = 0;
	PTZ_Motor_PID_Torque_Y.error_now = 0;
	PTZ_Motor_PID_Torque_Y.Kd = 0.8;
	PTZ_Motor_PID_Torque_Y.Ki = 0;
	PTZ_Motor_PID_Torque_Y.Kp = 3;

	//--------------------------云台电机pitch-----------------------------------------------------

	PTZ_Motor_PID_Position_P.error_inter = 0;
	PTZ_Motor_PID_Position_P.error_last = 0;
	PTZ_Motor_PID_Position_P.error_now = 0;
	PTZ_Motor_PID_Position_P.Kd = 1000;
	PTZ_Motor_PID_Position_P.Ki = 0.2;
	PTZ_Motor_PID_Position_P.Kp = 300;

	PTZ_Motor_PID_Speed_P.error_inter = 0;
	PTZ_Motor_PID_Speed_P.error_last = 0;
	PTZ_Motor_PID_Speed_P.error_now = 0;
	PTZ_Motor_PID_Speed_P.Kd = 0;
	PTZ_Motor_PID_Speed_P.Ki = 0;
	PTZ_Motor_PID_Speed_P.Kp = 20;

	PTZ_Motor_PID_Torque_P.error_inter = 0;
	PTZ_Motor_PID_Torque_P.error_last = 0;
	PTZ_Motor_PID_Torque_P.error_now = 0;
	PTZ_Motor_PID_Torque_P.Kd = 0;
	PTZ_Motor_PID_Torque_P.Ki = 0;
	PTZ_Motor_PID_Torque_P.Kp = 20;
	//--------------------------拨弹电机 2006- p96-----------------------------------------------------

#if DIALPOSITIONORSPEED==1

	Hit_position.error_inter = 0;
	Hit_position.error_last  = 0;
	Hit_position.error_last = 0;
	Hit_position.error_now = 0;	
	Hit_position.Kd = 0.12;
	Hit_position.Ki = 0.0125;
	Hit_position.Kp = 1.2;
	Hit_position.limit=32767;

	Hit_speed.error_inter =0;
	Hit_speed.error_last  =0;
	Hit_speed.error_last =0;
	Hit_speed.error_now = 0;
	Hit_speed.Kd = 0;
	Hit_speed.Ki = 0;
	Hit_speed.Kp = 401;
	Hit_speed.limit=32767;

#else

  	Hit_position.error_inter = 0;
	Hit_position.error_last  = 0;
	Hit_position.error_last = 0;
	Hit_position.error_now = 0;
	Hit_position.Kd = 200;
	Hit_position.Ki = 0;
	Hit_position.Kp = 10;
	Hit_position.limit=1000;

	Hit_speed.error_inter = 0;
	Hit_speed.error_last  = 0;
	Hit_speed.error_last = 0;
	Hit_speed.error_now = 0;
    Hit_speed.Kd = 0;
	Hit_speed.Ki = 0;
	Hit_speed.Kp = 2;
	Hit_speed.limit=32767;

#endif
}

void PID_Control_with_limit(float current, float expected, PID *motor_type)
{
	motor_type->error_last = motor_type->error_now;
	motor_type->error_now = expected - current;
	motor_type->error_inter += motor_type->error_now;

	//积分限幅
	if (motor_type->error_inter > motor_type->limit)
		motor_type->error_inter = motor_type->limit;
	if (motor_type->error_inter < -motor_type->limit)
		motor_type->error_inter = -motor_type->limit;

	motor_type->pid_out = motor_type->Kp * motor_type->error_now + motor_type->Ki * motor_type->error_inter +
						  motor_type->Kd * (motor_type->error_now - motor_type->error_last);

		//输出限幅，使输出值能被电机识别
	if (motor_type->pid_out > motor_type->limit)
		motor_type->pid_out = motor_type->limit;
	if (motor_type->pid_out < -motor_type->limit)
		motor_type->pid_out = -motor_type->limit;
}
void PID_Control(float current, float expected, PID *motor_type)
{
	motor_type->error_last = motor_type->error_now;
	motor_type->error_now = expected - current;
	motor_type->error_inter += motor_type->error_now;

	if (motor_type == &PTZ_Motor_PID_Torque_Y)
	{
		if (motor_type->error_inter > 29000)
			motor_type->error_inter = 29000;
		if (motor_type->error_inter < -29000)
			motor_type->error_inter = -29000;
	}
	else
	{
		if (motor_type->error_inter > 29000)
			motor_type->error_inter = 29000;
		if (motor_type->error_inter < -29000)
			motor_type->error_inter = -29000;
	}
	motor_type->pid_out = motor_type->Kp * motor_type->error_now + motor_type->Ki * motor_type->error_inter +
						  motor_type->Kd * (motor_type->error_now - motor_type->error_last);
	if (motor_type->pid_out > 29000)
		motor_type->pid_out = 29000;

	if (motor_type->pid_out < -29000)
		motor_type->pid_out = -29000;
}
